package flexo.math;

/**
 * Based on:
 * http://gpwiki.org/index.php/OpenGL:Tutorials:Using_Quaternions_to_represent_rotation
 */
public class Quaternion
{
	private static final int NUM_TEMP_QUATERNS = 3;
	private static Quaternion[] tempQuaterns;
	private static int tempIterator;
	
	static
	{
		tempQuaterns = new Quaternion[NUM_TEMP_QUATERNS];
		for (int i = 0; i < NUM_TEMP_QUATERNS; ++i)
		{
			tempQuaterns[i] = new Quaternion();
		}
		tempIterator = 0;
	}
	
	public static Quaternion getTempQuaterion()
	{
		if (tempIterator >= NUM_TEMP_QUATERNS)
		{
			tempIterator = 0;
		}
		
		return tempQuaterns[tempIterator++];
	}
	
	protected static final float PIOVER180 = (float)(Math.PI/180.0f);
	
	public float x,y,z,w;
	
	public Quaternion()
	{
		reset();
	}
	
	public Quaternion(Quaternion copy)
	{
		set(copy);
	}
	
	public void reset()
	{
		fromAxis(new Vector(1,0,0),0);
	}
	
	public void set(Quaternion q)
	{
		x = q.x;
		y = q.y;
		z = q.z;
		w = q.w;
	}
	
	public Quaternion(float x, float y, float z, float w)
	{
		this.x = x;
		this.y = y;
		this.z = z;
		this.w = w;
	}
	
	public Quaternion(Vector axis, float angle)
	{
		fromAxis(axis,angle);
	}
	
	public void conjugate()
	{
		normalize();
		
		x = -x;
		y = -y;
		z = -z;
	}
	
	/**
	 * Loads the Quaternion with a rotation using a vector/angle scheme.
	 * @param axis A normalized vector describing the axis of rotation. If it's not normalized, nothing will ocurr.
	 * @param angle The angle of rotation (in radians).
	 */
	public void fromAxis(Vector axis, float angle)
	{
		angle *= PIOVER180 * 0.5f;
		Vector vn = new Vector(axis);
		vn.normalize();
	 
		float sinAngle;
		sinAngle = (float)Math.sin(angle);
	 
		x = (vn.x * sinAngle);
		y = (vn.y * sinAngle);
		z = (vn.z * sinAngle);
		w = (float)Math.cos(angle);
	}
	
	public void rotate(Quaternion localRotation)
	{
		mul(localRotation,this,this);
	}
	
	public void normalize()
	{
		float magnitude = (float)Math.sqrt(w*w + x*x + y*y + z*z);
		w /= magnitude;
		x /= magnitude;
		y /= magnitude;
		z /= magnitude;
	}
	
	public static void mul(Quaternion op1, Quaternion op2, Quaternion result)
	{
		result.x = op1.w * op2.x + op1.x * op2.w + op1.y * op2.z - op1.z * op2.y;
		result.y = op1.w * op2.y + op1.y * op2.w + op1.z * op2.x - op1.x * op2.z;
		result.z = op1.w * op2.z + op1.z * op2.w + op1.x * op2.y - op1.y * op2.x;
		result.w = op1.w * op2.w - op1.x * op2.x - op1.y * op2.y - op1.z * op2.z;
	}
	
	public static Quaternion createRotationX(float angle)
	{
		return new Quaternion(new Vector(1,0,0),angle);
	}
	
	public static Quaternion createRotationY(float angle)
	{
		return new Quaternion(new Vector(0,1,0),angle);
	}
	
	public static Quaternion createRotationZ(float angle)
	{
		return new Quaternion(new Vector(0,0,1),angle);
	}
	
	public String toString()
	{
		return "x:" + x + ", y:" + y + ", z:" + z + ", w:" + w;
	}
}
